#include "pose_2d.h"

bool Pose2d::Plus(const double *x, const double *delta, double *x_plus_delta) const {
    x_plus_delta[0] = NormalizeAngle(x[0] + delta[0]);  
    x_plus_delta[1] = x[1] + delta[1];  
    x_plus_delta[2] = x[2] + delta[2]; 
    return true;
}

bool Pose2d::ComputeJacobian(const double *x, double *jacobian) const {
    Eigen::Map<Eigen::Matrix<double, 3, 3>> J(jacobian);
    J.setIdentity();
    return true;
}